/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "b2CircleShape.h"

bool b2CircleShape::TestPoint(const b2XForm& xf, const b2Vec2& p) const
{
	b2Vec2 center = b2Mul(xf, m_p);
	b2Vec2 d = p - center;
	return d.LengthSquared() <= (m_radius * m_radius);
}

b2SegmentCollide b2CircleShape::TestSegment(const b2XForm& transform, float32* lambda, b2Vec2* normal, const b2Segment& segment, float32 maxLambda) const
{
	b2Vec2 center = b2Mul(transform, m_p);
	b2Vec2 s = segment.p1 - center;
	float32 d = s.LengthSquared() - m_radius * m_radius;

	if(d < 0.0f)
	{
		*lambda = 0.0f;
		return b2_startInsideCollide;
	}

	// Solve quadratic equation.
	b2Vec2 r = segment.p2 - segment.p1;
	float32 c = b2Dot(s, r);
	float32 rr = r.LengthSquared();
	float32 sigma = c * c - rr * d;	//R*R - (S*sin(a)) * (S*sin(a))

	// check for negative discriminant and short segment.
	if((sigma<0.0f) || (rr<B2_FLT_EPSILON))
	{
		return b2_missCollide;	
	}

	// Find the intersection point.
	float32 a = -(c + b2Sqrt(sigma));
	
	float32 rLength = r.Length();
	if(((0.0f<=a) && (a<=maxLambda*rLength)))	//XY here i change the code. formal if (0.0f <= a && a <= maxLambda * rr) seems have bug.
	{
		a /= rLength;
		*lambda = a;
		*normal = s + a * r;
		normal->Normalize();
		return b2_hitCollide;
	}

	return b2_missCollide;
}

void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2XForm& transform) const 
{
	b2Assert(NULL != aabb);
	b2Vec2 center = b2Mul(transform, m_p);
	aabb->lowerBound.Set(center.x - m_radius, center.y - m_radius);
	aabb->upperBound.Set(center.x + m_radius, center.y + m_radius);
}

void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const 
{
	b2Assert(NULL != massData);
	massData->center = m_p;
	massData->mass = density * b2_pi * m_radius * m_radius;
	massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));
}

float32 b2CircleShape::ComputeSubmergedArea(const b2Vec2& normal, float32 offset, const b2XForm& xf, b2Vec2* c) const
{
	b2Vec2 center = b2Mul(xf, m_p);
	float32 l = -(b2Dot(normal, center) - offset);
	if(l<-m_radius + B2_FLT_EPSILON)
	{
		// Completely dry
		return 0;
	}
	if(l>m_radius)
	{
		// Completely wet.
		*c = center;
		return b2_pi * m_radius * m_radius;
	}

	//Magic
	float32 r2 = m_radius * m_radius;
	float32 l2 = l * l;
	float32 area = r2 * (asinf(l/m_radius) + b2_pi/2.0f)+ l * b2Sqrt(r2 - l2);

	//XY now i do not know how this works.
	float32 com = -2.0f/3.0f*pow(r2-l2,1.5f)/area;
	c->x = center.x + normal.x * com;
	c->y = center.y + normal.y * com;

	return area;
}
